#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include<ros/ros.h>
#include<geometry_msgs/Twist.h>
#include<iostream>



double vel_x = 0.0;
double vel_y = 0.0;
double imu_z = 0.0;

double vel_dt = 0.0;
double imu_dt = 0.0;

ros::Time last_loop_time(0.0);
ros::Time last_vel_time(0.0);
ros::Time last_imu_time(0.0);

ros::Publisher pubCmdvel;

void Cmd_Vel_Callback(const geometry_msgs::Twist& vel)
{
//	ROS_INFO("Received a /cmd_vel message!");
//	ROS_INFO("Linear Components:[%f,%f,%f]", cmd_vel.linear.x,cmd_vel.linear.y,cmd_vel.linear.z);
//	ROS_INFO("Angular Components:[%f,%f,%f]",cmd_vel.angular.x,cmd_vel.angular.y,cmd_vel.angular.z);
//	// Do velocity processing here:  
    // Use the kinematics of your robot to map linear and angular velocities into motor commands  
//    v_l = ...  
//    v_r = ...  
  
    // Then set your wheel speeds (using wheel_left and wheel_right as examples)  
//    wheel_left.set_speed(v_l)  
//    wheel_right.set_speed(v_r) 

	ros::Time current_time = ros::Time::now();

	vel_x = vel.linear.x;
	imu_z = vel.angular.z;
	
	vel_dt = (current_time - last_vel_time).toSec();
	imu_dt = vel_dt;
	last_vel_time = current_time;
	pubCmdvel.publish(vel);

}

int main(int argc, char** argv)
{

	double angular_scale,linear_scale;
    //定义一个消息发布者来发布“odom”消息，在定义一个tf广播，来发布tf变换信息
    ros::init(argc, argv, "odometry_publisher");
    ros::NodeHandle n;
	
    ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
	pubCmdvel  = n.advertise<geometry_msgs::Twist>("/SerialPort/cmd_vel", 50); //Create an IMU topic publisher //创建IMU话题发布者

	ros::Subscriber sub = n.subscribe("cmd_vel", 100, Cmd_Vel_Callback);
    tf::TransformBroadcaster odom_broadcaster;

	//n.getParam("angular_scale", angular_scale);
	//n.getParam("linear_scale", linear_scale);

	n.param<double>("linear_scale",    linear_scale,	1.0); 
    n.param<double>("angular_scale",   angular_scale,	1.0); 
    
    // 默认机器人的起始位置是odom参考系下的0点    
    double x = 0.0;
    double y = 0.0;
    double th = 0.0;

		
    // 让机器人的base_link参考系在odom参考系下以x轴方向0.1m/s，Y轴速度-0.1m/s，角速度0.1rad/s的状态移动
    //double vx = 0.1;
    //double vy = -0.1;
    //double vth = 0.1;
    
    //使用1Hz的频率发布odom消息，在实际系统中，往往需要更快的速度进行发布
    ros::Rate r(10);
	
    while(n.ok())
    {
        ros::spinOnce();              
		
		ros::Time current_time = ros::Time::now();

		double linear_velocity = vel_x;
		double angular_velocity = imu_z;
		
        //积分计算里程计信息 vy = 0;
  			//calculate angular displacement  θ = ω * t
		double delta_th = angular_velocity * imu_dt * angular_scale; //radians
		double delta_x = (linear_velocity * cos(th)) * vel_dt * linear_scale; //m
		double delta_y = (linear_velocity * sin(th)) * vel_dt * linear_scale; //m


		
        x += delta_x;
        y += delta_y;
        th += delta_th;
 
        //为了兼容二维和三维的功能包，让消息结构更加通用，里程计的偏航角需要转换成四元数才能发布，辛运的是，ROS为我们提供了偏航角与四元数相互转换的功能
        geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);
        //first, we'll publish the transform over tf
        geometry_msgs::TransformStamped odom_trans;
        odom_trans.header.stamp = current_time;
        odom_trans.header.frame_id = "odom";
        odom_trans.child_frame_id = "base_link";
        odom_trans.transform.translation.x = x;
        odom_trans.transform.translation.y = y;
        odom_trans.transform.translation.z = 0.0;
        odom_trans.transform.rotation = odom_quat;
 
        //send the transform
        odom_broadcaster.sendTransform(odom_trans);
 
        //next, we'll publish the odometry message over ROS
        nav_msgs::Odometry odom;
        odom.header.stamp = current_time;
        odom.header.frame_id = "odom";
 
        //set the position
        odom.pose.pose.position.x = x;
        odom.pose.pose.position.y = y;
        odom.pose.pose.position.z = 0.0;
        odom.pose.pose.orientation = odom_quat;
 
        //set the velocity
        odom.child_frame_id = "base_link";
        odom.twist.twist.linear.x = vel_x;
        odom.twist.twist.linear.y = vel_y;
        odom.twist.twist.angular.z = imu_z;
 
        //publish the message
        odom_pub.publish(odom);
        last_loop_time = current_time;
        r.sleep();
 
    }
}

